#include "CURControl.h"
#include "RTDEControl.h"
#include "ScriptClient.h"
#include "DashBoardClient.h"
#include "RTDEUtility.h"
#include <iostream>



namespace CURCONTROL {

CURControl::CURControl():
    dashboard_(nullptr),
    script_(nullptr),
    rtde_(nullptr),
    publishThread_(nullptr),
    running_(false),
    ctlCycleTimeMs_(8),
    enableFreedrive_(false)
{
}

CURControl::~CURControl()
{
    running_.store(false);
    if(publishThread_ && publishThread_->joinable()){
        publishThread_->join();
        delete publishThread_;
    }
}

#define FREEDRIVE_ENABLED 0x00020000

void CURControl::PublishThread()
{
    running_.store(true);
    std::shared_ptr<RTDEControl> rtde = this->rtde_;
    std::shared_ptr<ScriptClient> script = this->script_;
    std::shared_ptr<DashBoardClient> dashboard = this->dashboard_;
    bool preIsFreedrive = false;
    bool preDashConnected = dashboard->IsConnected();
    bool preScriptConnected = script->IsConnected();
    bool preRtdeConnected = rtde->IsConnected();
    bool isFirstConnect = true;// 首次链接标志
    RobotMode preRobotmode{RobotMode::ROBOT_MODE_UNKNOW};
    SafetyMode preSafetymode{SafetyMode::SAFETY_MODE_UNDEFINED_SAFETY_MODE};
    while (running_ && dashboard && script && rtde) {
        std::this_thread::sleep_for(std::chrono::milliseconds(ctlCycleTimeMs_));
        // 客户端链接状态检测----------------------start
        if(!dashboard->IsConnected()){// 断开状态
            if(preDashConnected && !isFirstConnect){// 断开信号
                std::string str{"dashboard client is disconnected!!!"};
                RTDEUtility::PrintStr(str);
            }
            std::this_thread::sleep_for(std::chrono::seconds(2));
            if(dashboard->Reconnect()){// 重连成功
                dashboard->EnableReceiveThread();
                std::string str{"dashboard client is reconnected!!!"};
                RTDEUtility::PrintStr(str);
            }
        }
        preDashConnected = dashboard->IsConnected();// 更新上一次dashboard链接状态

        if(resetError_.load() && dashboard->IsConnected()){
            int type = 3;
            this->ResetSafetyError(type);
        }
        if(!script->IsConnected()){
            if(preScriptConnected && !isFirstConnect){
                std::string str{"script client is disconnected!!!"};
                RTDEUtility::PrintStr(str);
                if(connectStatusChangedFunc_)
                    connectStatusChangedFunc_(false);
                rtde->Disconnect();
                dashboard->Disconnect();
            }
            std::this_thread::sleep_for(std::chrono::seconds(2));
            if(this->FilterSafetyRobotMode(12)){
                if(isFirstConnect){
                    if(script_->Connect(robotIp_)){
                        std::string str{"script client connected"};
                        RTDEUtility::PrintStr(str);
                    }
                }else{
                    if(script->Reconnect()){
                        script->EnableReceiveThread();
                        std::string str{"script client is reconnected!!!"};
                        RTDEUtility::PrintStr(str);
                    }
                }
            }
        }
        preScriptConnected = script->IsConnected();

        if(!rtde->IsConnected()){
            if(preRtdeConnected && !isFirstConnect){
                std::string str{"rtde client is disconnected!!!"};
                RTDEUtility::PrintStr(str);
            }
            std::this_thread::sleep_for(std::chrono::seconds(2));
            if(this->FilterSafetyRobotMode(12)){
                if(isFirstConnect){
                    if(rtde_->Connect(robotIp_)){
                        std::string str{"rtde client connected"};
                        RTDEUtility::PrintStr(str);
                    }
                }else{
                    if(rtde->Reconnect()){
                        std::string msg{"rtde client is reconnected!!!"};
                        RTDEUtility::PrintStr(msg);
                    }
                }
            }
        }
        preRtdeConnected = rtde->IsConnected();
        // 客户端链接状态检测----------------------end


        // rtde启动状态检测----------------------start
        if(rtde->IsConnected() && script->IsConnected()){
            if(!rtde->IsStarted()){// 未启动
                if(dashboard->GetSafetyMode() == SafetyMode::SAFETY_MODE_NORMAL &&
                        dashboard->GetRobotMode() >= RobotMode::ROBOT_MODE_POWER_OFF)// UR处于正常模式
                {
//                    printf("============================UR处于正常模式\n");
                    if(FilterSafetyRobotMode(24)){
//                        printf("===================================rtde->StartRTDE();\n");
                        script_->SendScript(SCRIPT_PATH);// 启动RTDE前先发送一次脚本目的是为了停止上一次RTDE的占用
                        std::this_thread::sleep_for(std::chrono::seconds(1));
                        rtde->StartRTDE();
                        std::this_thread::sleep_for(std::chrono::seconds(1));
                        rtde->SendClearCommand("");// 启动RTDE后清除命令寄存器的值
                        this->ctlCycleTimeMs_.store(1000 / rtde->GetFrequency());
                        std::string str{"机械臂控制频率 "};
                        str.append(std::to_string(ctlCycleTimeMs_.load()));
                        str.append("ms");
                        RTDEUtility::PrintStr(str);
                        if(connectStatusChangedFunc_){
                            isFirstConnect = false;
                            connectStatusChangedFunc_(true);
                        }
                    }
                }
            }
        }
        // rtde启动状态检测----------------------end

//        firstRun = false;
        if(this->freedriveFunc_){
            // 检测到正在示教或者freedrive
            if(rtde->GetDigitalInputBits() & FREEDRIVE_ENABLED ||
                    rtde->GetRobotStatus() == RobotStatus::ROBOT_STATUS_TEACH_BUTTON_PRESSED){
                if(!preIsFreedrive){// freedrive上升沿
                    this->freedriveFunc_(true);
                    preIsFreedrive = true;
                    if(enableFreedrive_.load()){
                        rtde->FreedriveMode();// 激活freedrive
//                        dashboard->StopProgram();
//                        std::this_thread::sleep_for(std::chrono::milliseconds(8));
//                        rtde->SendClearCommand("清除激活freedrive命令寄存器");
                    }
                }
            }else if(preIsFreedrive){// freedrive下降沿
                this->freedriveFunc_(false);
                preIsFreedrive = false;
//                rtde->EndFreedriveMode();// 关闭freedrive
//                std::this_thread::sleep_for(std::chrono::milliseconds(8));
//                rtde->SendClearCommand("清除freedrive命令寄存器");
            }
        }

        int safetymode = 0;
        int robotmode = 0;
        this->getStatus(&safetymode, &robotmode);

        if(safetymode != preSafetymode){// safetymode 发生变化
            if(modeChangedFunc_)
                modeChangedFunc_(0, static_cast<int>(safetymode), static_cast<int>(preSafetymode));
        }
        if(robotmode != preRobotmode){// robotmode 发生变化
            if(modeChangedFunc_)
                modeChangedFunc_(1, static_cast<int>(robotmode), static_cast<int>(preRobotmode));
        }

        preSafetymode = SafetyMode(safetymode);
        preRobotmode = RobotMode(robotmode);
    }
}

int CURControl::connectToRobot(const std::string &ip)
{
    this->robotIp_ = ip;
    if(!dashboard_){
        dashboard_ = std::make_shared<DashBoardClient>();
    }
    // 只对dashboard进行连接，因为script和rtde的连接有前提条件
    std::string msg;
    if(dashboard_->Connect(robotIp_, msg)){
        std::string str{"dashboard client connected"};
        RTDEUtility::PrintStr(str);
    }
    if(!script_){
        script_ = std::make_shared<ScriptClient>();
    }

    if(!rtde_){
        rtde_ = std::make_shared<RTDEControl>();
    }

    if(!publishThread_){
        publishThread_ = new std::thread(
                    std::bind(&CURControl::PublishThread, this));
    }
    return 0;
}

//bool CURControl::Reconnect()
//{
//    this->TryWaitForUrRunning();// 阻塞直到UR切换到running模式
//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
//    script_->SendScript(SCRIPT_PATH);
//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
//    return rtde_->Reconnect();
//}

int CURControl::disconnectToRobot()
{
    return 0;
}

int CURControl::setEnabled(bool enable, int)
{
    if(rtde_){
        rtde_->SetSendEnable(enable);
    }
    return 0;
}

int CURControl::getError(int *)
{
    return 0;
}

int CURControl::getStatus(int *safetymode, int *robotmode, int *)
{
    if(rtde_ && rtde_->IsStarted() && rtde_->IsConnected()){
        if(safetymode)
            *safetymode = static_cast<int>(rtde_->GetSafetyMode());
        if(robotmode)
            *robotmode = static_cast<int>(rtde_->GetRobotMode());
//        printf("safetymode取得是rtde=%d\n", *safetymode);
    }else if(dashboard_ && dashboard_->IsConnected()){
        if(safetymode)
            *safetymode = dashboard_->GetSafetyMode();
        if(robotmode)
            *robotmode = dashboard_->GetRobotMode();
//        printf("safetymode取得是dashboard=%d\n", *safetymode);
    }
    return 0;
}

int CURControl::isConnected()
{
    int error = -1;
    if((rtde_ && rtde_->IsConnected()) &&
            (dashboard_ && dashboard_->IsConnected()) &&
            (script_ && script_->IsConnected())){
        if(rtde_->IsStarted() /*&& rtde_->IsProgramRunning()*/) //保证emergyStopCheck正常运行
        {
            error = 0;
        }
    }
    return error;
}

int CURControl::getForce(double *)
{
    return 0;
}

int CURControl::setSpeedLevel(double)
{
    return 0;
}

long long CURControl::getDigitalInput()
{
    if(rtde_){
        return static_cast<long long>(rtde_->GetDigitalInputBits());
    }
    return 0;
}

long long CURControl::getDigitalOutput()
{
    if(rtde_){
        return static_cast<long long>(rtde_->GetDigitalOutputBits());
    }
    return 0;
}

int CURControl::getQdActual(double *)
{
    return 0;
}

int CURControl::getIActual(double *)
{
    return 0;
}

int CURControl::getToolVectorActual(std::vector<double> &pos)
{
    pos.clear();
    if(rtde_){
        std::vector<double> acPos = rtde_->GetActualTcpPose();
        for(unsigned long i = 0; i < 6; ++i){
            pos.push_back(acPos.at(i));
        }
    }
    return 0;
}

int CURControl::getTcpSpeedActual(double *)
{
    return 0;
}

int CURControl::getMotorTemperatures(double *)
{
    return 0;
}

int CURControl::getJointModes(double *)
{
    return 0;
}

int CURControl::getToolAccelerometerValues(double *)
{
    return 0;
}

double CURControl::getLinearMomentumNorm()
{
    return 0;
}

int CURControl::getVActual(double *)
{
    return 0;
}

double CURControl::getIRobot()
{
    return 0;
}

double CURControl::getVRobot()
{
    return 0;
}

double CURControl::getVMain()
{
    return 0;
}

double CURControl::getUrVersion()
{
    return 0;
}

int CURControl::turnOn(int)
{
    return 0;
}

int CURControl::turnOff()
{
    if(dashboard_){
        dashboard_->PowerOff();
    }
    return 0;
}

int CURControl::resetError(int type)
{
    this->resetError_.store(true);
    return 0;
}

int CURControl::getQActual(double *joint)
{
    if(rtde_){
        std::vector<double> j = rtde_->GetActualJoint();
        for(unsigned long i = 0; i < j.size(); ++i){
            joint[i] = j.at(i);
        }
    }
    return 0;
}

void CURControl::GetActualTcpForce(std::vector<double> &value)
{
    std::vector<double> t;
    value.swap(t);
    if(rtde_){
        value = rtde_->GetActualTcpForce();
    }
}

void CURControl::GetActualCurrent(std::vector<double> &value)
{
    std::vector<double> t;
    value.swap(t);
    if(rtde_){
        value = rtde_->GetActualCurrent();
    }
}

void CURControl::GetActualTemperatures(std::vector<double> &value)
{
    std::vector<double> t;
    value.swap(t);
    if(rtde_){
        value = rtde_->GetActualTemperatures();
    }
}

int CURControl::servoJ(double *p, double acc, double v, double t, double lookahead_time, int gain)
{
    if(!this->WaitForProgramRunning()){
        return -1;
    }
    if(rtde_){
        std::vector<double> joint(6);
        for(unsigned long i = 0; i < 6; ++i){
            joint[i] = p[i];
        }
        rtde_->ServoJ(joint, v, acc, t, lookahead_time, gain);
    }
    return 0;
}

int CURControl::MoveL(std::vector<double> pose, double acc, double v, double t, double r)
{
    return 0;
}

int CURControl::SpeedL(std::vector<double> pose, double acc, double t)
{
    return 0;
}

int CURControl::ServoJPlus(std::vector<double> pose,
                           double acc, double v, double t, double lookahead_time, int gain)
{
    if(!this->WaitForProgramRunning()){
        return -1;
    }
    if(rtde_){
        rtde_->ServoJ_POS(pose, v, acc, t, lookahead_time, gain);
    }
    return 0;
}

void CURControl::SetAccAndTime(double acc, double t)
{

}

void CURControl::setFreeDriveCallBack(std::function<void (bool)> f)
{
    this->freedriveFunc_ = f;
}

void CURControl::setConnectCallBack(std::function<void (bool)> f)
{
    connectStatusChangedFunc_ = f;
}

void CURControl::setStatusChangeCallBack(std::function<void (void *)> f)
{
    if(rtde_){
        rtde_->SetStatusChangedCallback(f);
    }
}

void CURControl::setModeChangeCallBack(std::function<void (int, int, int)> f)
{
    this->modeChangedFunc_ = f;
}

void CURControl::setToolVoltage(unsigned int v)
{
    if(!rtde_ || !script_){
        return;
    }
    char buf[256];
    sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
    if(v == 0){
        enableFreedrive_.store(false);
//        rtde_->SetToolVoltage(RTDEControl::ToolVoltage::TOOL_VOLTAGE_0_V);
//        std::this_thread::sleep_for(std::chrono::milliseconds(8));
//        rtde_->SendClearCommand("SetToolVoltage 0V");
//        RTDEUtility::PrintStr("SetToolVoltage 0V");
    }else if(v == 24){
        enableFreedrive_.store(true);
//        rtde_->SetToolVoltage(RTDEControl::ToolVoltage::TOOL_VOLTAGE_24_V);
//        std::this_thread::sleep_for(std::chrono::milliseconds(8));
//        rtde_->SendClearCommand("SetToolVoltage 24V");
//        RTDEUtility::PrintStr("SetToolVoltage 24V");
    }
    script_->SendCommand(buf);
}

void CURControl::setFlag(unsigned int n, bool b)
{

}

void CURControl::setDigitalOut(unsigned int n, bool b)
{
    if(rtde_){
        rtde_->SetStdDigitalOut(static_cast<uint8_t>(n), b);
    }
}

void CURControl::setAnalogOut(unsigned int n, double f)
{

}

bool CURControl::setPayload(double m)
{
    return true;
}

void CURControl::shutDown()
{

}

void CURControl::GetAllClientConnectedStatus(bool &dashboard, bool &primary, bool &secondary, bool &realtime)
{
    if(dashboard_){
        dashboard = dashboard_->IsConnected();
    }
    if(script_){
        primary = script_->IsConnected();
    }
    secondary = false;
    if(rtde_){
        realtime = rtde_->IsConnected();
    }
}

void CURControl::StopGently()
{
    if(rtde_){
        rtde_->StopServoThread();
        std::this_thread::sleep_for(std::chrono::milliseconds(ctlCycleTimeMs_.load()));
        rtde_->SendClearCommand("StopServoThread");
    }
}

int CURControl::DashboardControl(CURControl::DashBoardCommand command)
{
    if(!dashboard_){
        return -1;
    }
    switch (command) {
    case DashBoardCommand::POWER_ON:{
        if(dashboard_->PowerOn().find("Powering on") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::POWER_OFF:{
        if(dashboard_->PowerOff().find("Powering off") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::SHUTDOWN:{
        if(dashboard_->Shutdown().find("Shutting down") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::BRAKE_RELEASE:{
        if(dashboard_->BrakeRelease().find("Brake releasing") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::CLOSE_SAFETY_POPUP:{
        if(dashboard_->CloseSafetyPopup().find("closing safety popup") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::UNLOCK_PROTECTIVE_STOP:{
        if(dashboard_->UnlockProtectiveStop().find("Protective stop releasing") != std::string::npos)
            return 0;
        break;
    }
    case DashBoardCommand::RESTART_SAFETY:{
        if(dashboard_->RestartSafety().find("Restarting safety") != std::string::npos)
            return 0;
        break;
    }
    default:
        break;
    }
    return -1;
}

std::string CURControl::GetRobotMode() const
{
    if(dashboard_){
        return dashboard_->GetRobotModeStr();
    }
    return "";
}

void CURControl::StopScript()
{
    rtde_->StopScript();
}

void CURControl::StopJ()
{
    rtde_->StopJ();
}

bool CURControl::WaitForProgramRunning()
{
    int ms_count = 0;
    const int sleep_ms = 8;
    const int stabiliaztionCount = 4;
    int stabilizationTime = stabiliaztionCount;

    while (rtde_ && rtde_->IsConnected())
    {
        if(rtde_->IsProgramRunning()){
            return true;
        }
        if(!rtde_->IsStarted()){
            std::string msg{"WaitForProgramRunning() rtde is not start"};
            RTDEUtility::PrintStr(msg);
            break;
        }
        if(dashboard_->GetSafetyMode() != SafetyMode::SAFETY_MODE_NORMAL){
            std::string msg{"WaitForProgramRunning() safetymode is not normal"};
            RTDEUtility::PrintStr(msg);
            break;
        }
        if(stabilizationTime-- > 0){
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
            continue;
        }
        rtde_->SendClearServojReg();// 发送当前关节角度给寄存器
        std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
        rtde_->SendClearCommand("");
        // Wait for program to be running
        if(!script_->SendScript(SCRIPT_PATH)){
            break;
        }
        ++ms_count;
        stabilizationTime = stabiliaztionCount;
        if (ms_count > 100)
        {
            std::string error_str{"Failed to running rtde_script program"};
            RTDEUtility::PrintStr(error_str);
            throw std::logic_error(error_str);
        }
//        std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
    }
    return false;
}

void CURControl::TryWaitForUrRunning()
{
    int count = 10;
    while(dashboard_ && count > 0){
        std::this_thread::sleep_for(std::chrono::seconds(1));
        int safetymode = 0;
        int robotmode = 0;
        this->getStatus(&safetymode, &robotmode);
        if(safetymode == SafetyMode::SAFETY_MODE_SAFEGUARD_STOP ||
                safetymode == SafetyMode::SAFETY_MODE_SYSTEM_EMERGENCY_STOP ||
                safetymode == SafetyMode::SAFETY_MODE_ROBOT_EMERGENCY_STOP){
            continue;
        }
        if(safetymode == SafetyMode::SAFETY_MODE_NORMAL &&
                robotmode == RobotMode::ROBOT_MODE_RUNNING){
            break;
        }
        if(safetymode == SafetyMode::SAFETY_MODE_PROTECTIVE_STOP){
            dashboard_->CloseSafetyPopup();
            dashboard_->UnlockProtectiveStop();
        }
        else if(safetymode == SafetyMode::SAFETY_MODE_FAULT ||
                safetymode == SafetyMode::SAFETY_MODE_VIOLATION){
            dashboard_->CloseSafetyPopup();
            dashboard_->RestartSafety();
        }
        if(robotmode == RobotMode::ROBOT_MODE_IDLE){
            dashboard_->BrakeRelease();
        }
        if(robotmode == RobotMode::ROBOT_MODE_POWER_OFF){
            dashboard_->PowerOn();
        }
        --count;
    }
}

void CURControl::ResetSafetyError(int type)
{
    static bool isRestartSafeting = false;// 正在安全重启
    SafetyMode mode = dashboard_->GetSafetyMode();
    if(mode == SafetyMode::SAFETY_MODE_PROTECTIVE_STOP){
        //            if(!isUnlocking){
        dashboard_->CloseSafetyPopup();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        dashboard_->UnlockProtectiveStop();
        std::string str{"UnlockProtectiveStop"};
        //            }
    }
    else if(mode == SafetyMode::SAFETY_MODE_FAULT || mode == SafetyMode::SAFETY_MODE_VIOLATION){
        if(type == 3 && !isRestartSafeting){
            std::this_thread::sleep_for(std::chrono::seconds(3));
            dashboard_->CloseSafetyPopup();
            dashboard_->RestartSafety();
            isRestartSafeting = true;
            std::string str{"RestartSafety"};
            RTDEUtility::PrintStr(str);
            return;
        }
    }
    if(this->FilterSafetyRobotMode(12)){
        //            isUnlocking = false;
        isRestartSafeting = false;
        this->resetError_.store(false);
    }
}

bool CURControl::FilterSafetyRobotMode(int cycleCount)
{
    const int sleep_ms = 8;

    while (dashboard_ && dashboard_->IsConnected())
    {
        SafetyMode s = dashboard_->GetSafetyMode();
        if(s != SafetyMode::SAFETY_MODE_NORMAL){
//            printf("SafetyMode=%s\n", dashboard_->GetSafetyModeStr().c_str());
            return false;
        }
        RobotMode r = dashboard_->GetRobotMode();
        if(r < RobotMode::ROBOT_MODE_POWER_OFF){
//            printf("RobotMode=%s\n", dashboard_->GetRobotModeStr().c_str());
            return false;
        }
        if(cycleCount-- > 0){
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
            continue;
        }
        return true;
    }
    return false;
}

}// namespace CURCONTROL
